#include "AStar.h"

AStar::AStar(Point_d start_point, Point_d target_point, Oracle* oracle, CollisionDetector* col):
_start_point(start_point),
_target_point(target_point),
_oracle(oracle),
_col(col)
{
	AStarNode node;
	node.point = start_point;
	node.g_value = 0;
	node.f_value = estimation_function(start_point, target_point);
	node.valid = true;
	node.came_from = -1;

	_open_set.push_back(node);

	_target_set = get_conf_set(target_point);
}

vector<ConfSet> AStar::get_path()
{
	for (int i = 0; i < 20; ++i)
	{
		int current_best_node_index = get_current_best_node_index();

		if (current_best_node_index == -1)
		{
			break;
		}

		Point_d current_best_node = _open_set[current_best_node_index].point;

		if (_col->local_planner(get_conf_set(current_best_node),_target_set, 1))
		{
			return get_full_path(_open_set[current_best_node_index]);
		}

		_open_set[current_best_node_index].valid = false;
		_close_set.push_back(_open_set[current_best_node_index].point);

		vector<Point_d> neighbors = _oracle->get_legal_neighbors(current_best_node);
		for (vector<Point_d>::iterator it = neighbors.begin();
			it != neighbors.end();
			++it)
		{
			if (is_in_close_list(*it))
			{
				continue;
			}

			AStarNode node;
			node.point = *it;
			node.g_value = _open_set[current_best_node_index].g_value + 1;
			node.f_value = node.g_value + estimation_function(*it, _target_point);
			node.valid = true;
			node.came_from = current_best_node_index;

			_open_set.push_back(node);
		}
	}

	vector<ConfSet> empty_neighbors;
	return empty_neighbors;
}

vector<ConfSet> AStar::get_full_path(AStarNode node)
{
	vector<ConfSet> reversed_path;

	reversed_path.push_back(get_conf_set(_target_point));

	AStarNode current_node = node;

	while (current_node.came_from != -1)
	{
		reversed_path.push_back(get_conf_set(current_node.point));
		current_node = _open_set[current_node.came_from];
	}

	reversed_path.push_back(get_conf_set(_start_point));

	vector<ConfSet> path;
	
	for (int i = reversed_path.size() - 1; i >= 0; --i)
	{
		path.push_back(reversed_path[i]);
	}

	return path;
}

bool AStar::is_in_close_list(Point_d point)
{
	for (vector<Point_d>::iterator it = _close_set.begin();
		it != _close_set.end();
		++it)
	{
		if (*it == point)
		{
			return true;
		}
	}

	return false;
}

ConfSet AStar::get_conf_set(Point_d point)
{
	ConfSet configuration;

	Point_2 robot_1_conf(
		CGAL::to_double(point[0]),
		CGAL::to_double(point[1]));

	configuration.push_back(robot_1_conf);

	Point_2 robot_2_conf(
		CGAL::to_double(point[2]),
		CGAL::to_double(point[3]));

	configuration.push_back(robot_2_conf);

	return configuration;
}

double AStar::estimation_function(Point_d a, Point_d b)
{
    double diff0 = a[0] - b[0];
    double diff1 = a[1] - b[1];
    double diff2 = a[2] - b[2];
    double diff3 = a[3] - b[3];

    return ::sqrt(diff0 * diff0 + diff1 * diff1 + diff2 * diff2 + diff3 * diff3);
}

int AStar::get_current_best_node_index()
{
	double min_value = 0;
	int min_index = -1;

	bool was_init = false;

	int i = 0;

	for (; i < _open_set.size(); ++i)
	{
		if (_open_set[i].valid)
		{
			if (!was_init)
			{
				min_value = _open_set[i].f_value;
				min_index = i;
				was_init = true;
			}
			else
			{

				if (min_value > _open_set[i].f_value)
				{
					min_value = _open_set[i].f_value;
					min_index = i;
				}
			}
		}
	}

	return min_index;
}